#include "pclDeskewing.hpp"

ImuPreintegration::ImuPreintegration(int max_size) : max_size(max_size)
{
	rot_x.resize(max_size);
	rot_y.resize(max_size);
	rot_z.resize(max_size);
	time.resize(max_size);
	latest = -1;
}

bool ImuPreintegration::preintegrate(std::deque<sensor_msgs::msg::Imu> &imubuf)
{
	if (imubuf.size() < 1 || imubuf.size() > 100)
	{
		latest = -1;
		return false;
	}
	rot_x[0] = 0;
	rot_y[0] = 0;
	rot_z[0] = 0;
	time[0] = stamp_to_sec(imubuf[0].header.stamp);
	for (unsigned int i = 1; i < imubuf.size(); i++)
	{
		sensor_msgs::msg::Imu &imu = imubuf[i];
		double curtime = stamp_to_sec(imu.header.stamp);
		double dt = curtime - time[i - 1];
		//rot_x[i] = rot_x[i - 1] + imu.angular_velocity.x * dt;
		rot_x[i] = 0;
		//rot_y[i] = rot_y[i - 1] + imu.angular_velocity.y * dt;
		rot_y[i] = 0;
		rot_z[i] = rot_z[i - 1] + imu.angular_velocity.z * dt;
		time[i] = curtime;
	}
	latest = imubuf.size() - 1;
	return true;
}

bool ImuPreintegration::findRotation(double timepoint, double &x, double &y, double &z)
{
	if (latest < 0)
	{
		// no record
		x = 0;
		y = 0;
		z = 0;
		return false;
	}
	if (timepoint < time[0])
	{
		// timepoint earlier than all records
		x = rot_x[0];
		y = rot_y[0];
		z = rot_z[0];
		return false;
	}
	else if (timepoint > time[latest])
	{
		// timepoint later than all records
		x = rot_x[latest];
		y = rot_y[latest];
		z = rot_z[latest];
		return false;
	}
	else
	{
		// found nearest two records
		int front = (timepoint - time[0]) / (time[latest] - time[0]) * latest;
		int back = front - 1;
		double ratio_front = (timepoint - time[back]) / (time[front] - time[back]);
		double ratio_back = (time[front] - timepoint) / (time[front] - time[back]);
		x = rot_x[front] * ratio_front + rot_x[back] * ratio_back;
		y = rot_y[front] * ratio_front + rot_y[back] * ratio_back;
		z = rot_z[front] * ratio_front + rot_z[back] * ratio_back;
		return true;
	}
}
bool ImuPreintegration::findPosition(double timepoint, double &x, double &y, double &z)
{
	x = 0;
	y = 0;
	z = 0;
	return true;
}

bool ImuPreintegration::transformPointCloud(sensor_msgs::msg::PointCloud2 &pcl)
{
	pcl::PointCloud<PointIn> pcloud;
	pcl::fromROSMsg(pcl, pcloud);
	transformPointCloud(pcloud);
	pcl::toROSMsg(pcloud, pcl);
	return true;
}

bool ImuPreintegration::transformPointCloud(pcl::PointCloud<PointIn> &pcloud)
{
	double rx, ry, rz;
	double tx, ty, tz;
	findRotation(pcloud[0].timestamp, rx, ry, rz);
	findPosition(pcloud[0].timestamp, tx, ty, tz);
	auto tfb = pcl::getTransformation(tx, ty, tz, rx, ry, rz).inverse(); // basis transform
	PointIn po;
	// transform point cloud to start
	for (unsigned int i = 0; i < pcloud.size(); i++)
	{
		PointIn &pi = pcloud[i];
		findRotation(pi.timestamp, rx, ry, rz);
		findPosition(pi.timestamp, tx, ty, tz);
		Eigen::Affine3f tf = pcl::getTransformation(tx, ty, tz, rx, ry, rz);
		tf = tfb * tf; // transform from start to this point
		// product of tf*pi, equals pi*tf.inv()
		po.x = tf(0, 0) * pi.x + tf(0, 1) * pi.y + tf(0, 2) * pi.z + tf(0, 3);
		po.y = tf(1, 0) * pi.x + tf(1, 1) * pi.y + tf(1, 2) * pi.z + tf(1, 3);
		po.z = tf(2, 0) * pi.x + tf(2, 1) * pi.y + tf(2, 2) * pi.z + tf(2, 3);
		pi.x = po.x;
		pi.y = po.y;
		pi.z = po.z;
	}
	return true;
}

